#include "MEC_PID.h"
#include "Arduino.h"

MEC_PID::MEC_PID(float p, float d, float i)
{
	this->kP = p;
	this->kD = d;
	this->kI = i;
	
	output = 0;
	error = 0;
	errorSum = 0;
	errorOld = 0;
}

void MEC_PID::setParameter(float p, float d, float i)
{
	this->kP = p;
	this->kD = d;
	this->kI = i;
}

float MEC_PID::getOutput(float setPoint, float feeback, int dt)
{
	error = setPoint - feeback;
	
	//errorSum += error;
	iVal = 0;
	pVal = kP * error;	
	//iVal = kI * iVal + error*dt;
	dVal = kD * (error - errorOld) / dt;	
	output = pVal+iVal+dVal;	
	if(output > PI)
	{
		output = PI;
	}
	else if(output < 0)
	{
		output = 0;
	}
	errorOld = error;

	return output;
}